from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    pcl_file_path = LaunchConfiguration('pcl_file_path')
    path_file_path = LaunchConfiguration('path_file_path')
    min_pz_gain = LaunchConfiguration('min_pz_gain')
    max_pz_gain = LaunchConfiguration('max_pz_gain')
    flag_build_2d_map = LaunchConfiguration('flag_build_2d_map')
    map_file_path = LaunchConfiguration('map_file_path')
    grid_value_threshold = LaunchConfiguration('grid_value_threshold')
    voxel_leaf_size = LaunchConfiguration('voxel_leaf_size')
    ror_radius = LaunchConfiguration('ror_radius')
    ror_min_neighbors = LaunchConfiguration('ror_min_neighbors')

    map_manager_share = get_package_share_directory('map_manager')

    params = {
        'pcl_file_path': pcl_file_path,
        'path_file_path': path_file_path,
        'min_pz_gain': min_pz_gain,
        'max_pz_gain': max_pz_gain,
        'flag_build_2d_map': flag_build_2d_map,
        'grid_value_threshold': grid_value_threshold,
        'voxel_leaf_size': voxel_leaf_size,
        'ror_radius': ror_radius,
        'ror_min_neighbors': ror_min_neighbors
    }
    if map_file_path != '':
        params['map_file_path'] = map_file_path

    return LaunchDescription([
        DeclareLaunchArgument(
            'pcl_file_path',
            default_value='maps/scans.pcd',
            description='Path to the PCD file'
        ),
        DeclareLaunchArgument(
            'path_file_path',
            default_value='maps/path.csv',
            description='Path to the path CSV file'
        ),
        DeclareLaunchArgument(
            'flag_build_2d_map',
            default_value='true',
            description='Whether to build 2d map'
        ),
        DeclareLaunchArgument(
            'map_file_path',
            default_value='',
            description='Path to the 2d map PNG file'
        ),
        DeclareLaunchArgument(
            'min_pz_gain',
            default_value='1.3',
            description='Min pz gain'
        ),
        DeclareLaunchArgument(
            'max_pz_gain',
            default_value='1.0',
            description='Max pz gain'
        ),
        DeclareLaunchArgument(
            'grid_value_threshold',
            default_value='50.0',
            description='Grid value threshold for occupancy grid'
        ),
        DeclareLaunchArgument(
            'voxel_leaf_size',
            default_value='0.1',
            description='Voxel leaf size for downsampling'
        ),
        DeclareLaunchArgument(
            'ror_radius',
            default_value='0.05',
            description='Radius for ROR (Region of Relevance)'
        ),
        DeclareLaunchArgument(
            'ror_min_neighbors',
            default_value='30',
            description='Minimum number of neighbors for ROR'
        ),
        ExecuteProcess(
            cmd=[
                'rviz2',
                '-d', f'{map_manager_share}/config/2d_map.rviz'
            ],
            output='screen'
        ),
        Node(
            package='map_manager',
            executable='build_2d_map',
            name='build_2d_map_node',
            output='screen',
            parameters=[params]
        )
    ])